#include "ApproachFoodState.h"

#include "ReturnFoodState.h"
#include "FoodSearchState.h"

#include "AIAgent.h"
#include "SBSeek.h"
#include "FoodSource.h"
#include "SBSeparation.h"
#include "SBSlowOnApproach.h"
#include "SBFlee.h"
#include "FollowToFoodState.h"
#include "AntWorld.h"
#include "ControllablePredator.h"

#include <algorithm>


//Global for use with predicate
FoodSource* gFood = 0;

bool SortPredicate(AIAgent* __restrict agent1, AIAgent* __restrict agent2)
{

	Vec3f targetPos = gFood->GetPos();
	bool result = (targetPos - agent1->GetPos()).LengthSqr() < (targetPos - agent2->GetPos()).LengthSqr();
	return result;
}

ApproachFoodState::ApproachFoodState(AIAgent* __restrict agent, FoodSource* __restrict food) : AIState(agent), m_pFood(food)
{
	AntWorld* antWorld = TheAntWorld::Instance(); 
	
	Predator* pred = antWorld->GetRandPredator();
	ControllablePredator* contPred = antWorld->GetControlledPredator();

	m_behaviourMap.insert(BehaviourAndWeight(new SBSeek(agent, food), 1.0f));
	m_behaviourMap.insert(BehaviourAndWeight(new SBSeparation(agent, 3), 1.0f));
	m_behaviourMap.insert(BehaviourAndWeight(new SBSlowOnApproach(agent, food, 1.0f), 1.0f));

	m_behaviourMap.insert(BehaviourAndWeight(new SBFlee(agent, pred, 10.0f), 0.75f));
	m_behaviourMap.insert(BehaviourAndWeight(new SBFlee(agent, contPred, 10.0f), 0.75f));
	
	gFood = food;

	/*
	//SBFollow
	//Get neighbours
	std::vector<AIAgent*>* neighbours = agent->GetNeighbours();	
	//sort neighbours by distance to food
	std::sort(neighbours->begin(), neighbours->end(), SortPredicate); 
	//Iterate through setting follow order
	AIAgent* agentToFollow = agent;
	std::vector<AIAgent*>::iterator it = neighbours->begin();
	std::vector<AIAgent*>::iterator end = neighbours->end();
	for( ; it != end; ++it)
	{
		AIState* state = (*it)->GetAIState();
		//If the agent is looking for food
		if(state->GetStateName() == "FoodSearchState")
		{
			//Make it follow the previous agent that is following
			//state->InsertBehaviour(new SBFollow((*it), agentToFollow, 4), 1.0f);
		
			(*it)->SetAIState(new FollowToFoodState((*it), agentToFollow, m_pFood));
			agentToFollow = (*it);
		}
	}
	*/
	
}

/*
void ApproachFoodState::MakeFollowToFood(AIAgent* agentToFollow)
{
	m_behaviourMap.insert(new SBFollow(m_pAgent, agentToFollow), 1.0f);

}
*/


Vec3f ApproachFoodState::Update()
{
	Vec3f diff = m_pFood->GetPos() - m_pAgent->GetPos();
	if(m_pFood->IsDepleted())
	{
		
		m_pAgent->SetAIState(new FoodSearchState(m_pAgent));
		return Vec3f();
	}
	if(diff.LengthSqr() <  m_pFood->GetContactRadiusSqr())
	{
		//Change state
		m_pFood->RemoveNourishment();
		m_pAgent->SetAIState(new ReturnFoodState(m_pAgent));

		return Vec3f();
	}
	
	
	

	return SumForces();

}

const char* ApproachFoodState::GetStateName() const
{
	return "ApproachFood";
}

FoodSource* ApproachFoodState::GetFood()
{
	return m_pFood;
}

ApproachFoodState::~ApproachFoodState()
{

}
